#!/usr/bin/python3

import roslib
import rospy
import math
import tf
import tf2_ros
from geometry_msgs.msg import Pose, Twist, TransformStamped, PointStamped, PoseStamped
import time
import numpy as np
import csv
from std_msgs.msg import Float64MultiArray, Bool
from sensor_msgs.msg import LaserScan, JointState
from gazebo_msgs.msg import ModelStates
import copy
from tf2_geometry_msgs import tf2_geometry_msgs
import roslibpy
from digital_twining_bridge import twining_bridge

class p903_twining_bridge(twining_bridge):
    def __init__(self,nodename):
        rospy.init_node(nodename)
        # debug_prefix = "/p903_system/"
        debug_prefix = ""

        if(rospy.has_param(debug_prefix + "digital_twin_server_config")):
            digital_twin_server_config = rospy.get_param(debug_prefix + 'digital_twin_server_config')
        else:
            digital_twin_server_config = {}
            digital_twin_server_config['ip_address'] = '127.0.0.1'
            digital_twin_server_config['interface'] = 'rosbridge'
            digital_twin_server_config['system']= 'noetic'
        
        self.rosServer = None
        self.joint_name_index = None
        self.realtime_tracking_switch_on = True

        rosServer = self.getValidRosServer(digital_twin_server_config['ip_address'])
        self.sim_pogo_group_pub = roslibpy.Topic(rosServer,'/pogo_group/pogo_group_joint_states', 'sensor_msgs/JointState')
        self.sim_fine_machining_pub = roslibpy.Topic(rosServer,'/fine_machining/stewart_joint_states', 'sensor_msgs/JointState')

        self.pogo_group_joint_states = rospy.Subscriber("/pogo_group/joint_states",JointState,self.pogoGroupJointStates_cb)
        self.fine_machining_joint_states = rospy.Subscriber("/fine_machining/joint_states",JointState,self.fineMachiningJointStates_cb)
        # self.turn_on_realtime_tracking = rospy.Subscriber("turn_on_realtime_tracking",Bool, self.turnOnRealtimeTracking_cb)

        self.tfBroadcaster=tf2_ros.StaticTransformBroadcaster()
        self.staticTransformList = []

        if(rospy.has_param("key_components")):
            self.component_names = rospy.get_param("key_components")
            self.key_components = {}
            for i in self.component_names:
                self.key_components[i] = rospy.get_param(i+"/born")
        else:
            self.key_components = {}

        self.establish_key_components(self.key_components)

    def pogoGroupJointStates_cb(self,msg):
        self.republish_by_roslibpy(msg,self.sim_pogo_group_pub)
        return

    def fineMachiningJointStates_cb(self,msg):
        self.republish_by_roslibpy(msg,self.sim_fine_machining_pub)
        return

    def run(self):
        rospy.spin()

if __name__ == '__main__':
    controller = p903_twining_bridge('p903_twining_bridge')
    controller.run()
